# 附有条件的间接平差 Adjustment of observations with condition equations
from numpy import mat, array, deg2rad, sin, cos
from MathUtils import *

# 点名
A = 'A'
B = 'B'
C = 'C'
D = 'D'
E = 'E'
F = 'F'
G = 'G'

# 边名
AB = 'AB'
BC = 'BC'
CD = 'CD'
DF = 'DF'
FG = 'FG'
AG = 'AG'
AE = 'AE'
BE = 'BE'
CE = 'CE'
DE = 'DE'
EF = 'EF'
EG = 'EG'


# 前方交会
def forwardIntersection(A, B, alpha, beta):
	xa, ya = A
	xb, yb = B
	alpha = deg2rad(alpha)
	beta = deg2rad(beta)
	xp = (xa*cot(beta) + xb*cot(alpha) - ya + yb) / (cot(alpha) + cot(beta))
	yp = (ya*cot(beta) + yb*cot(alpha) + xa - xb) / (cot(alpha) + cot(beta))

	return array([xp, yp])


# 求边长
def calcDistance(P1, P2):
	return np.sqrt((P2 - P1).dot(P2 - P1))


# 误差方程系数
def coeA(s):
	rho = 206265
	return rho*sin(deg2rad(s[1]))/s[0]


def coeB(s):
	rho = 206265
	return -rho*cos(deg2rad(s[1]))/s[0]


np.set_printoptions(linewidth=400)
if __name__ == '__main__':
	# 点集合
	P = {}
	# 边集合
	S = {}
	# 角度观测值
	L = []

	# 读取角度观测值
	with open("OecAdj_AngleObservations.txt", "r") as f:
		while True:
			line = f.readline()
			if line == "":
				break
			d, m, s = line.split(",")
			d = float(d)
			m = float(m)
			s = float(s)
			L.append(dms2dec((d, m, s)))
		f.close()
	L = np.array(L)

	# 已知坐标
	P[A] = array([2794005.704, 19433831.155])
	P[B] = array([2802234.190, 19437826.220])

	# 计算近似坐标
	P[E] = forwardIntersection(P[A], P[B], L[1-1], L[2-1])
	P[C] = forwardIntersection(P[E], P[B], L[6-1], L[4-1])
	P[D] = forwardIntersection(P[E], P[C], L[9-1], L[7-1])
	P[F] = forwardIntersection(P[E], P[D], L[12-1], L[10-1])
	P[G] = forwardIntersection(P[E], P[F], L[15-1], L[13-1])

	print("近似坐标：")
	for p in P:
		print(p, P[p])

	# 计算近似边长和近似方位角
	S[AB] = [calcDistance(P[A], P[B]), azimuth(P[A], P[B])]
	S[BC] = [calcDistance(P[B], P[C]), azimuth(P[B], P[C])]
	S[CD] = [calcDistance(P[C], P[D]), azimuth(P[C], P[D])]
	S[DF] = [calcDistance(P[D], P[F]), azimuth(P[D], P[F])]
	S[FG] = [calcDistance(P[F], P[G]), azimuth(P[F], P[G])]
	S[AG] = [calcDistance(P[A], P[G]), azimuth(P[A], P[G])]
	S[AE] = [calcDistance(P[A], P[E]), azimuth(P[A], P[E])]
	S[BE] = [calcDistance(P[B], P[E]), azimuth(P[B], P[E])]
	S[CE] = [calcDistance(P[C], P[E]), azimuth(P[C], P[E])]
	S[DE] = [calcDistance(P[D], P[E]), azimuth(P[D], P[E])]
	S[EF] = [calcDistance(P[E], P[F]), azimuth(P[E], P[F])]
	S[EG] = [calcDistance(P[E], P[G]), azimuth(P[E], P[G])]

	print("近似边长和方位角：")
	for s in S:
		print(s, S[s])

	# Bmat = mat([
	# #	Xc	Yc	Xd	Yd	Xe	Ye	Xf	Yf	Xg	Yg
	# 	[0, 0, 0, 0, coeA(S[AE]), coeB(S[AE]), 0, 0, 0, 0],
	# 	[0, 0, 0, 0, -coeA(S[BE]), -coeB(S[BE]), 0, 0, 0, 0],
	# 	[0, 0, 0, 0, coeA(S[AE])-coeA(S[BE]), coeB(S[AE])-coeB(S[BE]), 0, 0, 0, 0],
	# 	[-coeA(S[BC]), -coeB(S[BC]), 0, 0, ]
	# ])

	# B矩阵列索引
	BCI = {
		'C': 0,
		'D': 2,
		'E': 4,
		'G': 6,
		'F': 8
	}

	Angs = [
		[AE, A, AB],
		[AB, B, BE],
		[BE, E, AE],
		[BE, B, BC],
		[BC, C, CE],
		[CE, E, BE],
		[CE, C, CD],
		[CD, D, DE],
		[DE, E, CE],
		[DE, D, DF],
		[DF, F, EF],
		[EF, E, DE],
		[EF, F, FG],
		[FG, G, EG],
		[EG, E, EF],
		[EG, G, AG],
		[AG, A, AE],
		[AE, E, EG]
	]

	Bmat = np.zeros((18, 10))
	Bmat_str = [
		['______', '______', '______', '______', '______', '______', '______', '______', '______', '______'],
		['______', '______', '______', '______', '______', '______', '______', '______', '______', '______'],
		['______', '______', '______', '______', '______', '______', '______', '______', '______', '______'],
		['______', '______', '______', '______', '______', '______', '______', '______', '______', '______'],
		['______', '______', '______', '______', '______', '______', '______', '______', '______', '______'],
		['______', '______', '______', '______', '______', '______', '______', '______', '______', '______'],
		['______', '______', '______', '______', '______', '______', '______', '______', '______', '______'],
		['______', '______', '______', '______', '______', '______', '______', '______', '______', '______'],
		['______', '______', '______', '______', '______', '______', '______', '______', '______', '______'],
		['______', '______', '______', '______', '______', '______', '______', '______', '______', '______'],
		['______', '______', '______', '______', '______', '______', '______', '______', '______', '______'],
		['______', '______', '______', '______', '______', '______', '______', '______', '______', '______'],
		['______', '______', '______', '______', '______', '______', '______', '______', '______', '______'],
		['______', '______', '______', '______', '______', '______', '______', '______', '______', '______'],
		['______', '______', '______', '______', '______', '______', '______', '______', '______', '______'],
		['______', '______', '______', '______', '______', '______', '______', '______', '______', '______'],
		['______', '______', '______', '______', '______', '______', '______', '______', '______', '______'],
		['______', '______', '______', '______', '______', '______', '______', '______', '______', '______'],
	]

	# 方位角改正数系数
	deltaAlpha = {
		AB: [0, 0],
		BC: [coeA(S[BC]), coeB(S[BC])],
		CD: [coeA(S[CD]), coeB(S[CD])],
		DF: [coeA(S[DF]), coeB(S[DF])],
		FG: [coeA(S[FG]), coeB(S[FG])],
		AG: [coeA(S[AG]), coeB(S[AG])],
		AE: [coeA(S[AE]), coeB(S[AE])],
		BE: [coeA(S[BE]), coeB(S[BE])],
		CE: [coeA(S[CE]), coeB(S[CE])],
		DE: [coeA(S[DE]), coeB(S[DE])],
		EF: [coeA(S[EF]), coeB(S[EF])],
		EG: [coeA(S[EG]), coeB(S[EG])],
	}
	print("方位角改正数系数：")
	for da in deltaAlpha:
		print(da, deltaAlpha[da])

	for A_line, B_line, BS_line in zip(Angs, Bmat, Bmat_str):
		station = A_line[1]
		targetStart = A_line[0].replace(station, '')
		targetEnd = A_line[2].replace(station, '')
		print(targetStart, station, targetEnd)

		if targetStart in BCI:
			B_line[BCI[targetStart]] = deltaAlpha[A_line[0]][0]
			B_line[BCI[targetStart] + 1] = deltaAlpha[A_line[0]][1]

			BS_line[BCI[targetStart]] = "a({})".format(A_line[0])
			BS_line[BCI[targetStart] + 1] = "b({})".format(A_line[0])

		if station in BCI:
			B_line[BCI[station]] = deltaAlpha[A_line[2]][0] - deltaAlpha[A_line[0]][0]
			B_line[BCI[station] + 1] = deltaAlpha[A_line[2]][1] - deltaAlpha[A_line[0]][1]

			BS_line[BCI[station]] = "a({})".format(A_line[2]) + "-" + "a({})".format(A_line[0])
			BS_line[BCI[station] + 1] = "b({})".format(A_line[2]) + "-" + "b({})".format(A_line[0])

		if targetEnd in BCI:
			B_line[BCI[targetEnd]] = -deltaAlpha[A_line[2]][0]
			B_line[BCI[targetEnd] + 1] = -deltaAlpha[A_line[2]][1]

			BS_line[BCI[targetEnd]] = "-" + "a({})".format(A_line[2])
			BS_line[BCI[targetEnd] + 1] = "-" + "b({})".format(A_line[2])

		if S[A_line[2]][1] - S[A_line[0]][1] < -180:
			A_line.append(S[A_line[2]][1] - S[A_line[0]][1] + 360)
		elif -180 <= S[A_line[2]][1] - S[A_line[0]][1] < 0:
			A_line.append(S[A_line[2]][1] - S[A_line[0]][1] + 180)
		elif S[A_line[2]][1] - S[A_line[0]][1] > 180:
			A_line.append(S[A_line[2]][1] - S[A_line[0]][1] - 180)
		else:
			A_line.append(S[A_line[2]][1] - S[A_line[0]][1])

	for line in Angs:
		print(line)

	for line in Bmat_str:
		for c in line:
			print("{: >16}".format(c), end="")
		print()

	print(Bmat)

	expand = mat([
		[-0.1816, 0.9834, 0.1816, -0.9834, 0, 0, 0, 0, 0, 0],
		[0, 0, 0, 0, 0.2555, -0.0962, 0, 0, 0, 0]
	])
	Bmat = np.r_[Bmat, expand]

	print(Bmat)
	print()
